import numpy as np
import matplotlib.pyplot as plt

# 模拟加速度传感器数据（带噪声的sin波）
np.random.seed(0)
t = np.linspace(0, 2, 200)  # 2秒，200个采样点
true_signal = np.sin(2 * np.pi * t)       # 理想信号
noise = np.random.normal(0, 0.2, t.shape) # 高斯噪声
sensor_data = true_signal + noise

# 一阶低通滤波函数
def low_pass_filter(data, alpha):
    filtered = np.zeros_like(data)
    filtered[0] = data[0]
    for i in range(1, len(data)):
        filtered[i] = (1 - alpha) * filtered[i-1] + alpha * data[i]
    return filtered

# 滤波
alpha = 0.05  # 滤波系数
filtered_data = low_pass_filter(sensor_data, alpha)

# 画图对比
plt.figure(figsize=(10,5))
plt.plot(t, sensor_data, label='Raw Sensor Data', alpha=0.6)
plt.plot(t, filtered_data, label='Low-pass Filtered', linewidth=2)
plt.plot(t, true_signal, label='True Signal', linestyle='--')
plt.xlabel('Time [s]')
plt.ylabel('Accel [m/s²]')
plt.title(f'Low-pass Filter Effect (alpha={alpha})')
plt.legend()
plt.grid(True)
plt.show()
